#include <cstdlib>
#include <cmath>

#include "TrackBall.h"

#ifndef PI
#define PI 3.1415926
#endif

#define DEG_TO_RAD(angle) (angle / 180 * PI)

namespace TrackBall {

void ProjectToSphere(int x, int y, int width, int height, float *pt)
{
    //transform x and y to range (-1, 1)
    pt[0] = (2.0 * x - width) / width;
    pt[1] = -(height - 2.0 * y) / height;

    float d = sqrt(pt[0] * pt[0] + pt[1] * pt[1]);
    //compute z
    if(d < 1)
    {
        pt[2] = sqrt(1 - d * d);
    }
    else
    {
        pt[2] = 0;
        pt[0] /= d;
        pt[1] /= d;
    }
}

float Rotation(float *lastPos, float *currentPos, float *axis)
{
    //compute rotation axis
    axis[0] = lastPos[1] * currentPos[2] - lastPos[2] * currentPos[1];
    axis[1] = lastPos[2] * currentPos[0] - lastPos[0] * currentPos[2];
    axis[2] = lastPos[0] * currentPos[1] - lastPos[1] * currentPos[0];

    //return rotation angle
    float dot = lastPos[0] * currentPos[0] + lastPos[1] * currentPos[1] + lastPos[2] * currentPos[2];
    return acos(dot > 1.0 ? 1.0 : dot) / 3.1415926 * 180;
}

float Rotation(int width, int height, int *lastPos, int *currentPos, float *axis)
{
    float LPos[3], CPos[3];
    ProjectToSphere(lastPos[0], lastPos[1], width, height, LPos);
    ProjectToSphere(currentPos[0], currentPos[1], width, height, CPos);

    float angle = Rotation(LPos, CPos, axis);

    return angle;
}

void RotationMatrix(float angle, float *axis, float *m)
{
    float d = sqrt(axis[0] * axis[0] + axis[1] * axis[1] + axis[2] * axis[2]);
    float x = axis[0] / d;
    float y = axis[1] / d;
    float z = axis[2] / d;

    float COS = cos(DEG_TO_RAD(angle));
    float SIN = sin(DEG_TO_RAD(angle));

    m[0] = x * x * (1 - COS) + COS;
    m[1] = x * y * (1 - COS) + z * SIN;
    m[2] = x * z * (1 - COS) - y * SIN;
    m[3] = 0;

    m[4] = x * y * (1 - COS) - z * SIN;;
    m[5] = y * y * (1 - COS) + COS;
    m[6] = y * z * (1 - COS) + x * SIN;
    m[7] = 0;

    m[8] = x * z * (1 - COS) + y * SIN;
    m[9] = y * z * (1 - COS) - x * SIN;
    m[10] = z * z * (1 - COS) + COS;
    m[11] = 0;

    m[12] = m[13] = m[14] = 0;
    m[15] = 1;
}

void MultMatrix(const float *in, float *out)
{
    float tmp[16] = {0};
    for(int i = 0; i < 4; ++i)
    {
        for(int j = 0; j < 4; ++j)
        {
            tmp[i * 4 + j] +=out[i * 4 + 0] * in[j];
            tmp[i * 4 + j] +=out[i * 4 + 1] * in[4 + j];
            tmp[i * 4 + j] +=out[i * 4 + 2] * in[8 + j];
            tmp[i * 4 + j] +=out[i * 4 + 3] * in[12 +j];
        }
    }

    for(int i = 0; i < 4; ++i)
    {
        for(int j = 0; j < 4; ++j)
            out[i * 4 + j] = tmp[i * 4 + j];
    }
}

void GetQ(const float angle, const float *axis, float *Q)
{
    float d = sqrt(axis[0] * axis[0] + axis[1] * axis[1] + axis[2] * axis[2]);
    float x = axis[0] / d;
    float y = axis[1] / d;
    float z = axis[2] / d;
    Q[0] = cos(DEG_TO_RAD(angle / 2));
    Q[1] = sin(DEG_TO_RAD(angle / 2)) * x;
    Q[2] = sin(DEG_TO_RAD(angle / 2)) * y;
    Q[3] = sin(DEG_TO_RAD(angle / 2)) * z;
}

void RotationMatrix(const float *Q, float *m)
{
    float w = Q[0];
    float x = Q[1];
    float y = Q[2];
    float z = Q[3];

    m[0] = 1 - 2 * y * y - 2 * z * z;
    m[1] = 2 * x * y + 2 * w * z;
    m[2] = 2 * x * z - 2 * w * y;
    m[3] = 0;

    m[4] = 2 * x * y - 2 * w * z;
    m[5] = 1 - 2 * x * x - 2 * z * z;
    m[6] = 2 * y * z + 2 * w * x;
    m[7] = 0;

    m[8] = 2 * x * z + 2 * w * y;
    m[9] = 2 * y * z - 2 * w * x;
    m[10] = 1 - 2 * x * x - 2 * y * y;
    m[11] = 0;

    m[12] = m[13] = m[14] = 0;
    m[15] = 1;
}

void AccumulateQ(const float *pre, const float *cur, float *out)
{
    out[0] = pre[0] * cur[0] - pre[1] * cur[1] - pre[2] * cur[2] - pre[3] * cur[3];
    out[1] = pre[0] * cur[1] + pre[1] * cur[0] + pre[2] * cur[3] - pre[3] * cur[2];
    out[2] = pre[1] * cur[2] - pre[1] * cur[3] + pre[2] * cur[0] + pre[3] * cur[1];
    out[3] = pre[0] * cur[3] + pre[1] * cur[2] - pre[2] * cur[1] + pre[3] * cur[0];

    float mag = sqrt(out[0] * out[0] + out[1] * out[1] + out[2] * out[2] + out[3] * out[3]);
    out[0] /= mag;
    out[1] /= mag;
    out[2] /= mag;
    out[3] /= mag;
}

}
